
using Tao.OpenGl;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using System;

namespace cs5643.rigidbody
{




    /**
     * Maintains a dynamic list of RigidBody objects, and provides access
     * to their state for numerical integration of dynamics and collision
     * processing.
     * 
     * @author Doug James, March 2007.
     */
    public class RigidBodySystem
    {
        /** Current simulation time. */
        double time = 0;

        /** List of RigidBody objects. */
        HashSet<RigidBody> bodies = new HashSet<RigidBody>();

        /** List of Force objects. */
        List<Force> F = new List<Force>();

        CollisionProcessor collisionProcessor = null;
        bool processCollisions = true;

        //List<Constraint> C = new List<Constraint>();

        /** Basic constructor. */
        public RigidBodySystem() { }

        /** Adds a force object (until removed) */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void addForce(Force f)
        {
            F.Add(f);
        }

        /** Useful for removing temporary forces, such as user-interaction
         * spring forces. */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void removeForce(Force f)
        {
            F.Remove(f);
        }

        /** Fragile reference to rigid bodies.  */
        public ICollection<RigidBody> getRigidBodies()
        {
            return bodies;
        }

        /** Number of rigid bodies. */
        public int getNBodies() { return bodies.Count; }

        /** Picks body based on some criteria, or null if none picked.  */
        public RigidBody pickBody(Pnt2D p)
        {
            double pickDist = Double.MaxValue;
            RigidBody pick = null;
            foreach (RigidBody body in bodies)
            {

                if (body.isPinned()) continue;
                Pnt2D pdist = Pnt2D.Sub(body.getPosition(),p);
                double dist = pdist.Length;
                if (dist < pickDist)
                {
                    pickDist = dist;
                    pick = body;
                }
            }
            return pick;
        }

        /** Adds the RigidBody to the system, and invalidates the existing
         * CollisionProcessor. */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void add(RigidBody rb)
        {
            bodies.Add(rb);

            /// INVALIDATE CollisionProcessor (needs to be rebuilt at timestep):
            collisionProcessor = null;
        }


        /** Removes the RigidBody from the system, and invalidates the
         * existing CollisionProcessor. */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void remove(RigidBody rb)
        {
            bodies.Remove(rb);

            /// INVALIDATE CollisionProcessor (needs to be rebuilt at timestep):
            collisionProcessor = null;
        }

        /** Moves all rigidbodys to undeformed/materials positions, and
         * sets all velocities to zero. Synchronized to avoid problems
         * with simultaneous calls to advanceTime(). */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void reset()
        {
            foreach (RigidBody rb in bodies)
            {
                rb.reset();
                //rb.setHighlight(false);
            }
            time = 0;
        }

        /** Applies some random accelerations to the objects (modify as
         * desired). */
        public void jiggleON()
        {
            jiggle = true;
        }

        private bool jiggle = false;

        private void applyjiggleON()
        {
            Pnt2D f = new Pnt2D(0, 0);//-->nonzero
            double tau = 0;
            double amp = 10000;///BUG
            foreach (RigidBody body in bodies)
            {
                double m = body.getMass();
                double I = body.getMassAngular();
                Random r = new Random();
                f.x = amp * m * (r.NextDouble() - 0.5);
                f.y = amp * m * (r.NextDouble() - 0.5);
                tau = 10 * amp * I * (r.NextDouble() - 0.5);
                body.applyWrenchW(f, tau);
            }
        }

        /**
         * Incomplete/Debugging integrator implementation. 
         * 
         * TODO: Modify this function to implement the integrator based on
         * the velocity-level complementarity constraint solver.
         */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void advanceTime(double dt)
        {
            /// NOTE: Clear force accumulators: already done after time step in RigidBody

            {/// Gather forces: (TODO)

                if (jiggle)
                {
                    applyjiggleON();
                    jiggle = false;
                }

                foreach (Force force in F) force.applyForce();

                // GRAVITY + CHEAP DAMPING:
                Pnt2D f = new Pnt2D(0, 0);//-->nonzero
                double tau = 0;
                foreach (RigidBody body in bodies)
                {
                    f.x = f.y = 0;
                    double m = body.getMass();
                    double I = body.getMassAngular();

                    /// DAMPING:
                    double C = 0.1;
                    
                    //Utils.acc(f, -C * m, body.getVelocityLinear());//linear damping
                    f.ScaleAdd( body.getVelocityLinear(),( -C * m));

                    tau = -C / 5.0 * I * body.getVelocityAngular();//angular damping

                    f.y -= m * 10;//gravity

                    body.applyWrenchW(f, tau);
                }
            }

            /// RESOLVE COLLISIONS!
            {
                if (collisionProcessor == null)
                {//BUILD
                    collisionProcessor = new CollisionProcessor(bodies);
                }
                if (processCollisions) collisionProcessor.processCollisions();
            }

            /// "ADVANCE TIME" SHOULD BE COMBINED WITH CONTACT SOLVER ...
            foreach (RigidBody body in bodies)
            {
                body.advanceTime(dt);
            }

            time += dt;
        }

        /** Enables/disables collision processing. */
        public void setProcessCollisions(bool enable)
        {
            processCollisions = enable;
        }
        /** Returns true if collision processing is enabled, and false
         * otherwise. */
        public bool getProcessCollisions()
        {
            return processCollisions;
        }


        /**
         * Displays RigidBody and Force objects.
         */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void display()
        {
            foreach (RigidBody body in bodies)
            {
                body.display();
                // sometimes bodies don't show up, or blocks in bodies, i blame Tao.OpenGl
                // and just call it twice to be safe
                //body.display();
            }
            foreach (Force force in F)
            {
                force.display();
            }
        }

    }
}